#include "camera_capture.h"

using namespace std;
using namespace cv;
using namespace cv_camera_capture;

void CameraCapture::front_camera() {
#if 0
    struct timespec time1 = {0, 0};
    clock_gettime(CLOCK_REALTIME, &time1);
    ROS_WARN("front_camera start_time: %ld.%ld s", time1.tv_sec, time1.tv_nsec);

    double start_time = static_cast<double>(cv::getTickCount());
    double time_end = ((double)cv::getTickCount() - start_time) / cv::getTickFrequency();
    ROS_WARN("front_camera time_end: %f s", time_end);
#endif

    cv::VideoCapture capture_front;
    if(!capture_front.open(front_camera_id_, cv::CAP_V4L2)) {
		ROS_ERROR("open camera %d error", front_camera_id_);
        return;
	}

    image_transport::ImageTransport front_image_transport(nh_front_camera_);
    image_transport::Publisher publish_front_image = front_image_transport.advertise(front_image_output_, 1);
    sensor_msgs::ImagePtr msg_front_image;

    int count = 0;
    bool state;
    cv::Scalar image_sum;
    cv::Mat image_src;

    while(ros::ok()) {
        front_camera_mutex_.lock();
        state = front_camera_state_;
        front_camera_mutex_.unlock();
		if(state == false) {
			usleep(100 * 1000);
            //ROS_WARN("front_camera_state false");
			continue;
		}

        if(!capture_front.grab()) {
			ROS_ERROR("camera %d grab error", front_camera_id_);
			usleep(100 * 1000);
			continue;
		}
        if(!capture_front.retrieve(image_src)) {
            ROS_ERROR("camera %d retrieve error", front_camera_id_);
			usleep(100 * 1000);
            continue;
        }
#if 1
		if(count > 5) {
            count = 0;
            image_sum = cv::sum(image_src(Rect(0, 0, image_src.rows/4, image_src.cols/4)));
            if(image_sum[0] < 1) {
			    ROS_ERROR("camera %d image_sum: %f, get frame error", front_camera_id_, 1.0*image_sum[0]);
			    usleep(100 * 1000);
			    continue;
            }
		}
        count++;
#endif
        //发布图像
        msg_front_image = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_src).toImageMsg();
        publish_front_image.publish(msg_front_image);
    }

    return;
}

void CameraCapture::back_camera() {
    cv::VideoCapture capture_back;
    if(!capture_back.open(back_camera_id_, cv::CAP_V4L2)) {
		ROS_ERROR("open camera %d error", back_camera_id_);
        return;
	}

    image_transport::ImageTransport back_image_transport(nh_back_camera_);
    image_transport::Publisher publish_back_image = back_image_transport.advertise(back_image_output_, 1);
    sensor_msgs::ImagePtr msg_back_image;

    int count = 0;
    bool state;
    cv::Scalar image_sum;
    cv::Mat image_src;

    while(ros::ok()) {
        back_camera_mutex_.lock();
        state = back_camera_state_;
        back_camera_mutex_.unlock();
		if(state == false) {
			usleep(100 * 1000);
            //ROS_WARN("back_camera_state false");
			continue;
		}
		
		if(!capture_back.grab()) {
			ROS_ERROR("camera %d grab error", back_camera_id_);
			usleep(100 * 1000);
			continue;
		}

        if(!capture_back.retrieve(image_src)) {
            ROS_ERROR("camera %d retrieve error", back_camera_id_);
			usleep(100 * 1000);
            continue;
        }
#if 1
		if(count > 5) {
            count = 0;
            image_sum = cv::sum(image_src(Rect(0, 0, image_src.rows/4, image_src.cols/4)));
            if(image_sum[0] < 1) {
			    ROS_ERROR("camera %d image_sum: %f, get frame error", back_camera_id_, 1.0*image_sum[0]);
			    usleep(100 * 1000);
			    continue;
            }
		}
        count++;
#endif
        //发布图像
        msg_back_image = cv_bridge::CvImage(std_msgs::Header(), "bgr8", image_src).toImageMsg();
        publish_back_image.publish(msg_back_image);
    }

    return;
}

void CameraCapture::camera_state_msg(const std_msgs::Bool::ConstPtr &msg, int camera_select) {
    struct timespec time0 = {0, 0};
    switch(camera_select) {
        case FRAME_CAMERA_SELECT:
            front_camera_mutex_.lock();
            front_camera_state_ = msg->data;
            front_camera_mutex_.unlock();
            break;

        case BACK_CAMERA_SELECT:
            back_camera_mutex_.lock();
            back_camera_state_ = msg->data;
            back_camera_mutex_.unlock();
            break;

        default:
            break;
    }
}

void CameraCapture::front_camera_msg(const std_msgs::Bool::ConstPtr &msg) {
    front_camera_state_ = msg->data;
    ROS_WARN("front_camera_state %d ok", front_camera_state_);
}

void CameraCapture::get_param()
{
	nh_front_camera_.getParam("front_camera_state", front_camera_state_);
	nh_front_camera_.getParam("front_camera_id", front_camera_id_);
	nh_front_camera_.getParam("front_camera_rate", front_camera_rate_);
	nh_front_camera_.getParam("front_image_width", front_image_width_);
	nh_front_camera_.getParam("front_image_height", front_image_height_);
    nh_front_camera_.getParam("front_image_output", front_image_output_);
	nh_front_camera_.getParam("get_front_camera_state", get_front_camera_state_);

	nh_back_camera_.getParam("back_camera_state", back_camera_state_);
	nh_back_camera_.getParam("back_camera_id", back_camera_id_);
	nh_back_camera_.getParam("back_camera_rate", back_camera_rate_);
	nh_back_camera_.getParam("back_image_width", back_image_width_);
	nh_back_camera_.getParam("back_image_height", back_image_height_);
    nh_back_camera_.getParam("back_image_output", back_image_output_);
	nh_back_camera_.getParam("get_back_camera_state", get_back_camera_state_);
}

void CameraCapture::run_subscribe()
{
    //设定订阅主题
    sub_front_camera_ = nh_front_camera_.subscribe<std_msgs::Bool>(get_front_camera_state_, 1, boost::bind(&CameraCapture::camera_state_msg, this, _1, FRAME_CAMERA_SELECT));
    sub_back_camera_ = nh_back_camera_.subscribe<std_msgs::Bool>(get_back_camera_state_, 1, boost::bind(&CameraCapture::camera_state_msg, this, _1, BACK_CAMERA_SELECT));

    //sub_front_camera_ = nh_front_camera_.subscribe(get_front_camera_state_, 1, &CameraCapture::front_camera_msg, this);
}

CameraCapture::CameraCapture()
    : nh_front_camera_("~"), nh_back_camera_("~")
{
    get_param();
    run_subscribe();
}

CameraCapture::~CameraCapture()
{

}
